import math
import pygame
import sys
import copy
from pprint import pprint
from math import pi
from math import sqrt
from collections import deque
import time
predTot = 0
predFail = 0
first = 0
level = None
tempWalls = {}
com_enemy_shot = {} # enemies who will be shot by teammate
com_ammo_nav = {} # ammo points which will be taken by teammate
com_ammo_av = {} # ammo points currently available to teammates
com_enemy_vis = [] # enemies visible (and if theyre near a CP/their location) (CPidNearLast, lastCoords, camping?, roundLastSeen)
com_agents = [] # (ammo, controlpointID, camping (0 = on way, 1 = camping, 2 = leave spot, 3 = no camping), camperspot, location, targetScore, lastTarget)
#com_agents[observation['id'] - 1]
com_controlPoints = [0, 0, 0] #0 = neutral, 1 = ours, 2 = ours + at least 1 in sight,
# 3 is ours + defending (=sitting on it), 4 = ours + defending with ammo
# 5 = 3 + helper, 5 = 3+helper w ammo
#-1 is taken by opponent, -2 is taken and defended on spot, -3 + ammo
com_controlAgentNearP = [0, 0, 0] # whether one of our agent could see this point last round
com_controlPScore = [350, 350, 350] # heuristic scores for the control points
# TODO: make a reasonable heuristic score ;). Higher score -> more reason to go there
# current idea: -decrease if it's ours and agent is near? decrease max 100, if close + point is ours
#               -increase if it's neutral/theirs for longer time? negatory, just keep it.
#                -Increase if enemy spotted (50 per uniq enemy/turn?)
##com_locationsVisited = [[70,70, -15], [70, 730, -15], [730, 70, -15], [730, 730, -15], [730, 490, -15], [ 730, 310, -15], [200, 330, -15], [200, 470, -15], [90, 400, -15]] # points to visit to keep track of
com_locationsVisited = [[50,50,-15],[50,150,-15],[50,250,-15],[50,350,-15],[50,450,-15],[50,550,-15],[50,650,-15],[50,750,-15],
[150,50,-15],[150,150,-15],[150,250,-15],[150,350,-15],[150,450,-15],[150,550,-15],[150,650,-15],[150,750,-15],
[250,50,-15],[250,150,-15],[250,250,-15],[250,350,-15],[250,450,-15],[250,550,-15],[250,650,-15],[250,750,-15],
[350,50,-15],[350,150,-15],[350,250,-15],[350,350,-15],[350,450,-15],[350,550,-15],[350,650,-15],[350,750,-15],
[450,50,-15],[450,150,-15],[450,250,-15],[450,350,-15],[450,450,-15],[450,550,-15],[450,650,-15],[450,750,-15],
[550,50,-15],[550,150,-15],[550,250,-15],[550,350,-15],[550,450,-15],[550,550,-15],[550,650,-15],[550,750,-15],
[650,50,-15],[650,150,-15],[650,250,-15],[650,350,-15],[650,450,-15],[650,550,-15],[650,650,-15],[650,750,-15],
[750,50,-15],[750,150,-15],[750,250,-15],[750,350,-15],[750,450,-15],[750,550,-15],[750,650,-15],[750,750,-15] ]


MAX_SPEED = 40
MAX_SHOT_DISTANCE  = 60
MAX_SIGHT = 100
AGENT_SIZE = 5 # radius
CONTROLPOINT_SIZE = 10 #radius

WEIGHT_AM_BASE = 350
WEIGHT_AM_AMMO = 5
WEIGHT_AM_MOVES = 3
WEIGHT_BESTCP_PREV = 50
WEIGHT_ENEMY_SBonus = 30
team = -1
round = 0
GRID_SIZE = 10
INIT_ROUND = 4

tempWalls = []
heatmap = [[{'a':0,'d':0} for c in range(800/GRID_SIZE)] for r in range(800/GRID_SIZE)]

campers = {}
camppoints = []

class AgentBrain():
    def __init__(self):
        self.goalID = -1 # it will get a new goal later on anyway
        self.goalScore = 100
        self.expQueue = []
        self.path = []
        self.lastHeuristicDecrease = 0
        self.hist = deque([])
        self.ammoGoal = (-1, -1)
        self.ammoGSteps = 999
        self.enemyGoal = -1 # id of enemy target to shoot in 1/2/3 turns
        self.enemyGSteps = 999
        self.nextL = (-1, -1)
        self.prevp = []
        self.lastType = ''
        self.predPrevL = (-1, -1)
        self.prevob = {}
        self.prevob['agents'] = []
        self.death = 1
    def action(self, observation):
        global first, level, com_enemy_shot, om_ammo_nav, com_controlPScore
        global round, first, MAX_goto_flag, directions, com_agents, com_controlPoints
        global com_enemy_nav, com_controlAgentNearP, com_enemy_vis
        global team, tempWalls, heatmap, campers, camppoints
        if first == 0:
            team = observation['team']
            first = observation['id']
            level = ReadLevel('levels\\clover2.py')
            for i in range(8):
                com_agents.append([0,-1,0,0,(-1,-1),-1,(-1,-1)])
                com_enemy_vis.append([-1,(-1,-1),-1,0])

        level.createTree()
        tempWalls = {}

        if first == observation['id']:
            round += 1
            #com_enemy_shot = {}
            #level = ReadLevel('levels\\clover2.py')

            #Remove temp nodes
            #level.tree.cleanMap()

            processCPObservation(observation['controlpoints'])

        loc = observation['location']

        if observation['respawn']:
            if (com_agents[observation['id'] - 1][4] >= (0,0)):
                r = com_agents[observation['id'] - 1][4][0] // GRID_SIZE
                c = com_agents[observation['id'] - 1][4][1] // GRID_SIZE
                heatmap[r][c]['d'] += 5
            for a in self.prevob['agents']:
                if a['team'] != observation['team']:
                    if a['location'] in campers:
                        campers[a['location']] += 1
                    else:
                        campers[a['location']] = 0
                    if campers[a['location']] == 2:
                        x1 = a['location'][0] - MAX_SHOT_DISTANCE // GRID_SIZE
                        x2 = a['location'][0] + MAX_SHOT_DISTANCE // GRID_SIZE
                        y1 = a['location'][1] - MAX_SHOT_DISTANCE // GRID_SIZE
                        y2 = a['location'][1] + MAX_SHOT_DISTANCE // GRID_SIZE
                        x1 = 0 if x1 < 0 else x1
                        x2 = 800/GRID_SIZE if x2 > 800/GRID_SIZE else x2
                        y1 = 0 if y1 < 0 else y1
                        y2 = 800/GRID_SIZE if y2 > 800/GRID_SIZE else y2
                        for i in range(x1, x2):
                            for j in range(y1, y2):
                                heatmap[i][j] += 20
                        camppoints += [(x1, y1), (x1, y2), (x2, y1), (x2, y2)]
            com_agents[observation['id'] - 1] = [0, -1, 0, 0, loc, -1, (-1, -1)]
            self.goalID = -1
            self.prevp = []
            self.death = round
            # TODO: add prev spot to camper spot?
            #print '%i omfg we got killed at %s' % (round, str(self.prevP.coord))
        else:
            com_agents[observation['id'] - 1][4] = loc

        if str(self.ammoGoal) in com_ammo_nav and self.ammoGSteps == com_ammo_nav[str(self.ammoGoal)]:
            del com_ammo_nav[str(self.ammoGoal)] # was ours; delete (probably picking again if it's still there)
        if self.enemyGoal in com_enemy_shot and self.enemyGSteps == com_enemy_shot[self.enemyGoal]:
            del com_enemy_shot[self.enemyGoal] # was ours; delete (probably picking again if it's still near)
        self.enemyGoal = -1
        self.enemyGSteps = 999

        preNav(loc,observation)

        (sh,a,d, enemyID) = shoot(loc,observation)

        if sh == 0:
            prevGoal = self.goalID
            if prevGoal != -1: # we had a controlpoint goal last time
                com_controlPScore[prevGoal] = com_controlPScore[prevGoal] + self.lastHeuristicDecrease

            (self.goalID, self.goalScore, heurCPDist) = bestCPGoal(loc,prevGoal,observation) # id and score
            (amScore, bestAmmo, bestMovesReq, lowestID) = ammo(loc,observation)
            curGoal = ''

            ammoHeur = min(40, observation['ammo'] * observation['ammo'] * 2)
            self.lastHeuristicDecrease = max(60 + ammoHeur - (heurCPDist + 1) * heurCPDist, 10)

            #Compare prio's ammo/controlpoints
            if amScore > self.goalScore:
                curGoal = bestAmmo
                self.ammoGoal = curGoal
                com_ammo_nav[str(bestAmmo)] = bestMovesReq # teammates need a bit less to take it instead
                self.ammoGSteps = bestMovesReq
                com_agents[observation['id'] - 1][5] = amScore
                #if lowestID != -1:
                    #print 'do something muttley!!' # increase that com_locationsVisited a bit?
                    #print '%i %s unvisited target score: %i target: %s' % (observation['id'] - 1, loc,amScore, curGoal)

            else:
                com_agents[observation['id'] - 1][5] = self.goalScore
                #print 'score: %i %i target: %s' % (self.goalScore, com_controlPScore[self.goalID], level.controls[self.goalID])
                curGoal = getFreeCampSpot(self.goalID, observation['id'] - 1, curGoal)

            com_agents[observation['id'] - 1][6] = curGoal

            if self.goalID != -1:
                if com_controlPoints[self.goalID] <= 0 and self.lastHeuristicDecrease > 40:
                    self.lastHeuristicDecrease = 40
                com_controlPScore[self.goalID] = com_controlPScore[self.goalID] - self.lastHeuristicDecrease
                #    com_controlPScore[self.goalID] -= 2 # TODO: fix... improve heuri decrease to prevent teammates to go there too

            #print "%i %s bCP: %i %i bA: %s %i hD: %i" % (observation['id'], str(com_controlPScore), self.goalID, self.goalScore, bestAmmo, amScore, self.lastHeuristicDecrease)
            if (round - self.death) < INIT_ROUND:
                curGoal = (615,615)
            
            (a, d) = self.moveToGoal(loc, curGoal, observation)

        elif sh == 1: # shooting..
            com_agents[observation['id'] - 1][0] = observation['ammo'] - 1 # more accurate update of ammo
            enCPid = com_enemy_vis[enemyID-1][0]
            if -1 != enCPid:
                com_controlPScore[enCPid] = com_controlPScore[enCPid] - WEIGHT_ENEMY_SBonus
                if com_controlPoints[enCPid] < -1:
                    com_controlPoints[enCPid] = -1
            com_enemy_vis[enemyID-1] = [-1,(-1,-1), 0,round]
            self.enemyGSteps = 0
            self.enemyGoal = enemyID
            com_enemy_shot[enemyID] = 0 # shooting in '0' moves
        else: # gonna shoot in 2-turns
            sh = 0
            com_enemy_shot[enemyID] = 2 #TODO: check if this can be 3 or 4 too..
            self.enemyGoal = enemyID
            self.enemyGSteps = 2
        action = {
            'turn': a,
            'speed': d,
            'shoot': sh
        }
        self.prevob['agents'] = observation['agents']
        return action

    def moveToGoal(self, loc, curGoal, observation):
        global com_agents, level, tempWalls
        if (round - self.death) >= INIT_ROUND:
            for agent in observation['agents']:
                al = agent['location']
                #Prevent goal blocking in path planning
                if (dist(curGoal, al) > 10):
                    level.tree.addSplit(al[0]+11,al[1]+11)
                    level.tree.addSplit(al[0]+11,al[1]-11)
                    level.tree.addSplit(al[0]-11,al[1]+11)
                    level.tree.addSplit(al[0]-11,al[1]-11)

                    tk = (((al[0]-10,al[1]-10),(al[0]+10,al[1]+10)),((al[0]-10,al[1]+10),(al[0]+10,al[1]-10)))
                    tl = []
                    tl.append(((al[0]+5,al[1]+10),(al[0]+10,al[1]+5)))
                    tl.append(((al[0]+10,al[1]+5),(al[0]+10,al[1]-5)))
                    tl.append(((al[0]+10,al[1]-5),(al[0]+5,al[1]-10)))
                    tl.append(((al[0]+5,al[1]-10),(al[0]-5,al[1]-10)))
                    tl.append(((al[0]-5,al[1]-10),(al[0]-10,al[1]-5)))
                    tl.append(((al[0]-10,al[1]-5),(al[0]-10,al[1]+5)))
                    tl.append(((al[0]-10,al[1]+5),(al[0]-5,al[1]+10)))
                    tl.append(((al[0]-5,al[1]+10),(al[0]+5,al[1]+10)))
                    tempWalls[tk] = tl
        for p in camppoints:
            level.tree.addSplit(p[0], p[1])
        path = []
        node = self.theta_star(loc, curGoal)
        #Handling group None-path issues (Tim)
        if node is None:
            print '%s no path found  to %s :/ ' % (str(loc), str(curGoal))
            #time.sleep(2.5)
            #showlevel(loc, curGoal)
            return (pi/3,20)
        else:
            while(node.parent != node):
                path.append(node.loc())
                node = node.parent
        if path == []:
            #print '%i %i %i empty path; looks like we will go camp?' %  (round, self.goalID, observation['id'] - 1)
            com_agents[observation['id'] - 1][2] = 1
            a = getPrefDirection(curGoal)
            d = 0
            #if self.goalID != -1 and com_agents[observation['id'] - 1][3] == 1 and ammoHeur > 20:
                #com_controlPScore[self.goalID] -= 1 # we are the 'head-camper'
        else:
            d = dist(loc,path[-1])
            a = angle(loc,path[-1])

        a -= observation['direction']
        if abs(a) > math.pi:
            if a >= 0:
                a -= 2*math.pi
            else:
                a += 2*math.pi
        if a > math.pi / 3:
            d = 0
            a = math.pi / 3
        if a < - math.pi / 3:
            d = 0
            a =  - math.pi / 3
        
        ap = a + observation['direction']
        if ap > math.pi:
            ap -= 2*math.pi
        elif ap < -math.pi:
            ap += 2*math.pi
            
        # TODO: check when we should move to get somewhere faster
        if path != [] and d < MAX_SPEED:
            #if self.prevp != []: # lame check to see if location predicted correctly
                #global predTot
                #predTot += 1
                #if abs(self.nextL[0] - loc[0]) > 1 or abs(self.nextL[1] - loc[1]) > 1:
                    #global predFail
                    #predFail += 1
                    #print 'C%s L%s Pr%s t: %s' % (loc, self.predPrevL, self.nextL, self.lastType) 
                    #print 'fail/total: %i %i' % (predFail, predTot)
            nextX = loc[0] # predicted next X/Y
            nextY = loc[1]
            maxX = loc[0] # 40 speed X/Y
            maxY = loc[1]
            if abs(ap) >= 0.5*math.pi: # going to the 'east'
                apt = ap
                if ap < 0:
                    apt = 0 - ap
                apt -= 0.5 * math.pi    
                nextX -= math.sin(apt) * d
                maxX -= math.sin(apt) * MAX_SPEED
                if ap < 0: # going 'north'
                    nextY -= math.cos(apt) * d
                    maxY -= math.cos(apt) * MAX_SPEED
                    self.lastType = '1, ap: %f apt: %f d: %f' % (ap/pi, apt/pi, d)
                else: # 'south'
                    nextY += math.cos(apt) * d
                    maxY += math.cos(apt) * MAX_SPEED
                    self.lastType = '2, ap: %f apt: %f d: %f' % (ap/pi, apt/pi, d)
            else: # going to the 'west' 
                apt = abs(ap)
                nextX += math.cos(apt) * d
                maxX += math.cos(apt) * MAX_SPEED
                if ap <= 0: # going 'north'
                    nextY -= math.sin(apt) * d
                    maxY -= math.sin(apt) * MAX_SPEED
                    self.lastType = '3, ap: %f apt: %f d: %f' % (ap/pi, apt/pi, d)
                else: # 'south'
                    nextY += math.sin(apt) * d
                    maxY += math.sin(apt) * MAX_SPEED
                    self.lastType = '4, ap: %f apt: %f d: %f' % (ap/pi, apt/pi, d)
            self.nextL = ( int(nextX), int(nextY))
            self.predPrevL = loc
            
            maxPos = ( int(maxX), int(maxY)) 
            # see if moving to maxPos makes us closer to one of next target in path
            maxS = False
            for i in range(-10,0):
                if abs(i) <= len(path):
                    cD = dist( self.nextL, path[i])
                    mpD = dist( maxPos, path[i])
                    if mpD < cD and line_of_sight(maxPos, path[i]):
                        maxS = True
            
            if maxS:
                #print '%s MAX_SPEED instead of %i!!' % (loc, d)
                d = MAX_SPEED

        self.prevp = path

        #Determine total movement w.r.t. history
        dt = 0
        #For each point in the history
        for i in self.hist:
            #Increase the total distance
            dt += dist(i, loc)
            #Add current point to history

        self.hist.append(loc)
        #If length of history > 2
        if len(self.hist) > 2:
            #Pop 1 element from history
            self.hist.popleft()

        if dt < 1 and len(self.hist) == 2 and dist(loc, curGoal) > 5: #If total movement < 1: Stuck, make random turn
            a = 2* pi / 9 # random.random()*pi*2/9 + pi/9
            d = 30
            self.prevp = []
        return (a, d)

    def theta_star(self, start, goal, verbose = False):
        global level
        self.expQueue = []
        closed = []
        s_start = level.tree.addSplit(start[0], start[1])
        s_start.g = 0
        s_start.parent = s_start
        s_goal = level.tree.addSplit(goal[0], goal[1])

        s_start.val = cost(s_start, s_goal)
        self.expQueue.append(s_start)
        while len(self.expQueue):
            s = self.expQueue.pop(0)
            if s == s_goal:
                return s
            closed.append(s)
            for k in s.neigh:
                if not s.neigh[k] is None: #TEMP fix attempt (if + indent 9 lines) (nout)
                    if verbose == True:
                        print s.neigh[k].loc()
                    s_1 = s.neigh[k]
                    if not line_of_sight(s.loc(), s_1.loc()):
                        continue
                    if not (s_1 in closed):
                        if not (s_1 in self.expQueue):
                            s_1.g = 100000
                            s_1.parent = None
                        self.update_point(s, s_1, s_goal)
        return None

    def update_point(self, s, s_1, s_goal):
        g_old = s_1.g
        compute_cost(s, s_1)
        if s_1.g < g_old:
            if not s_1 in self.expQueue:
                self.expQueue.append(s_1)
            s_1.val = s_1.g + cost(s_1, s_goal)
            self.expQueue.sort()

#Theta star
#Theta star
def compute_cost(s, s_1):
    if line_of_sight(s.parent.loc(), s_1.loc()):
        if s.parent.g + cost(s.parent, s_1) < s_1.g:
            s_1.parent = s.parent
            s_1.g = s.parent.g + cost(s.parent, s_1)
    else:
        if s.g + cost(s, s_1) < s_1.g:
            s_1.parent = s
            s_1.g = s.g + cost(s, s_1)

def cost(s, s_1):
    pixels = line(s.loc(), s_1.loc())
    tcost = 0
    for p in pixels:
        tcost += heatmap[p[0]][p[1]]['d']
    return tcost + sqrt((s_1.x - s.x)*(s_1.x - s.x)+(s_1.y - s.y)*(s_1.y - s.y))

def line_of_sight(s, s_1):
    for wall in level.walls:
        w = wall[0]
        w_1 = wall[1]
        if ((w_1[1]-s[1])*(w[0]-s[0]) > (w[1]-s[1])*(w_1[0]-s[0])) != ((w_1[1]-s_1[1])*(w[0]-s_1[0]) > (w[1]-s_1[1])*(w_1[0]-s_1[0])) \
            and ((w[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w[0]-s[0])) != ((w_1[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w_1[0]-s[0])):
            return False
    for wallx in tempWalls:
        flag = False
        for wall in wallx:
            w = wall[0]
            w_1 = wall[1]
            #
            if ((w_1[1]-s[1])*(w[0]-s[0]) > (w[1]-s[1])*(w_1[0]-s[0])) != ((w_1[1]-s_1[1])*(w[0]-s_1[0]) > (w[1]-s_1[1])*(w_1[0]-s_1[0])) \
                and ((w[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w[0]-s[0])) != ((w_1[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w_1[0]-s[0])):
                flag = True
                break
        if flag:
            #print 'tempWall match %s' % (str(wall))
            for wall in tempWalls[wallx]:
                w = wall[0]
                w_1 = wall[1]
                if ((w_1[1]-s[1])*(w[0]-s[0]) > (w[1]-s[1])*(w_1[0]-s[0])) != ((w_1[1]-s_1[1])*(w[0]-s_1[0]) > (w[1]-s_1[1])*(w_1[0]-s_1[0])) \
                    and ((w[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w[0]-s[0])) != ((w_1[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w_1[0]-s[0])):
                    return False
    return True


def line_of_sight_old(s, s_1):
    for wall in level.walls:
        w = wall[0]
        w_1 = wall[1]
        if ((w_1[1]-s[1])*(w[0]-s[0]) > (w[1]-s[1])*(w_1[0]-s[0])) != ((w_1[1]-s_1[1])*(w[0]-s_1[0]) > (w[1]-s_1[1])*(w_1[0]-s_1[0])) \
            and ((w[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w[0]-s[0])) != ((w_1[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w_1[0]-s[0])):
            return False
    return True

# Returns the grid points giving an approximation of the line between s and s1
def line(s, s_1):
    x0 = (s[0] // GRID_SIZE)
    y0 = (s[1] // GRID_SIZE)
    x1 = (s_1[0] // GRID_SIZE)
    y1 = (s_1[1] // GRID_SIZE)

    dx = abs(x1-x0)
    dy = abs(y1-y0)
    sx = 1 if x0 < x1 else -1
    sy = 1 if y0 < y1 else -1
    
    err = dx-dy
    
    pixels = []
    while x0 != x1 or y0 != y1: 
        pixels.append((x0, y0))
        e2 = 2 * err
        if e2 > -dy:
            err = err - dy
            x0 = x0 + sx
        if e2 <  dx:
            err = err + dx
            y0 = y0 + sy
    pixels.append((x0, y0))
    return pixels


#Navigation
def preNav(loc,observation):
    # some pre Navigation steps..
    # go over all visible agents to see if any of them occupy enemy controlpoint
    # only a controlpoint which we can see needs to be checked (current map it's max 1 same time)
    global com_controlPoints, com_locationsVisited, com_controlAgentNearP, com_agents, com_enemy_vis, com_controlPScore
    global com_ammo_av, com_locationsVisited
    id = observation['id']
    cpId = -1
    gP = '' # the goalPoint we can see (if there is one)
    if com_agents[id-1][2] == 1: # we were camping; reset
        com_agents[id-1][2] = 0

    com_agents[id-1][0] = observation['ammo'] # more accurate update of ammo

    #exit()
    for i in range(3): # for all controlpoints
        #com_controlPoints
        conP = level.controls[i]
        #gPt = Point(controlP[0], controlP[1],0,False)
        d = dist(loc, conP)
        if d <= MAX_SIGHT:
            cpId = i
            gP = conP

            #if com_controlPoints[cpId] == 0 or com_controlPoints[cpId] == 1:
                #self.path.appendleft(gP) # neutral point close by.. let's reset to see if we should get/defend this
                #resetting_path = cpId

            if d <= MAX_SIGHT - CONTROLPOINT_SIZE:   # so that we can see the full point
                com_controlAgentNearP[cpId] = 1
                if com_controlPoints[cpId] == -2: # last state it was theirs + occupied
                    com_controlPoints[cpId] = -1 # set it to just 'theirs' and see if it's occupied next

                if com_controlPoints[cpId] > 0 and line_of_sight(loc, conP): #standing near it AND it's ours
                    if com_controlPoints[cpId] == 1: # previously wasn't someone on/near
                        com_controlPoints[cpId] = 2
                    #print '%i standing and owning %s' % (id, str(conP))
                    if com_agents[id-1][2] != 2: # aren't running to make spot so should go camping
                        #TODO: add check for teammate camping here?
                        #print '%i NEW/Reached goal %i %i' % (id, controlP[0], controlP[1])
                        com_agents[id-1][2] = 1 # we're now camping?
                        #self.goalInt = cpId
                        #self.camping = 1

                    if d <= CONTROLPOINT_SIZE: # we are standing on the controlpoint
                        if com_controlPoints[cpId] > 0: #standing on it AND it's ours
                            if com_controlPoints[cpId] == 2: # previously wasn't someone on
                                com_controlPoints[cpId] = 3
                            #print '%i standing and owning %s' % (id, str(conP))
                            if observation['ammo'] > 0:
                                com_controlPoints[cpId] = 4
                    #else: # we are standing on CP but it's not ours.. let's move!
    if com_agents[id-1][2] != 1:
        #print 'not camping! (anymore?)'
        com_agents[observation['id'] - 1][3] = 0

    dell = {} # update ammo, remove old ones we will see first
    for ammos in com_ammo_av:
        if dist(loc, eval(ammos)) < MAX_SIGHT:
            dell[ammos] = 1
    for ammos in dell:
        del com_ammo_av[ammos]
    for ammo_av in observation['ammopacks']: # update ammo_av with current observation
        if ammo_av['location'][0] > 3 and ammo_av['location'][0] < 798 \
                and ammo_av['location'][1] > 3 and ammo_av['location'][1] < 798:
            com_ammo_av[str(ammo_av['location'])] = round

    for agent in observation['agents']:
        if agent['team'] != team: # for now we only care about the opponents
            #if str(agent['location']) in com_enemy_nav: %i enemy %s getting shot!
            enID = agent['id'] - 1
            ep = (agent['location'][0],agent['location'][1])
            if cpId >= 0:
                if com_enemy_vis[enID][0] != cpId:
                    com_enemy_vis[enID][0] = cpId
                    com_controlPScore[cpId] = com_controlPScore[cpId] + WEIGHT_ENEMY_SBonus # extra weight to this point cuz there is an enemy to beat..
                    #print 'EXTRA WEIGHT %i' % cpId # TODO: remove weight if shooting this enemy?

                if com_controlPoints[cpId] == -1: # check if an enemy is on this spot
                    d = dist(gP, ep)
                    if d <= CONTROLPOINT_SIZE:
                        com_controlPoints[cpId] = -2 # taken + defended
                #TODO: check in how many turns we can kill this enemy and put the enemy in a prio list to shoot
            if com_enemy_vis[enID][3] != round:
                com_enemy_vis[enID] = [cpId,ep, com_enemy_vis[enID][1] == ep,round]

    for locations in com_locationsVisited:
        [x, y, score] = locations
        if dist(loc, (x, y)) < 50: # TODO, get a more logical check here?
            locations[2] = round # currently checks if it's next to this point but point
            #print 'we are at %i %i (for ammo checking) new score: %i' % (x, y, round)
            # where it should go to and point where it checks should perhaps be different
    return


def shoot(loc,observation):
    global com_enemy_shot
    if observation['ammo'] == 0:
        return (0,0,0, -1)
    #TODO: CHOOSE BEST TARGET; the one requiring least turns? or double-shot target?
    sa = 0
    sd = 0
    sh = 0
    sID = 0
    for agent in observation['agents']:
        if agent['team'] != observation['team']:
            if (agent['id'] not in com_enemy_shot or com_enemy_shot[agent['id']] > 0) \
                    and line_of_sight_old(loc,agent['location']):
                a = angle(loc,agent['location'])
                d = dist(loc,agent['location'])

                a -= observation['direction']
                if abs(a) > math.pi:
                    if a >= 0:
                        a -= 2*math.pi
                    else:
                        a += 2*math.pi
                # if direction is roughly same -> en runs moves away from us
                dirSame = (1 / 3) * math.pi < abs(agent['direction'] - observation['direction'])
                #TODO: R U CAMPING? IS THIS GOOD SHOOTING BEHAVIOUR?
                if abs(a) <= math.pi / 3 and (int(math.ceil(d / MAX_SHOT_DISTANCE)) - 1) <= 0:
                    #print 'SHOOTING!!'
                    if com_agents[observation['id']-1][2] == 1 and com_agents[observation['id']-1][3] == 1: # we're 'head-camper' (and on spot?)
                        return (1, a, 0, agent['id']) # no need to check other enemies, perhaps move aswell tho?
                    return (1, a, 40, agent['id'])
                if (agent['id'] not in com_enemy_shot):
                    sh = 2
                    if com_agents[observation['id']-1][2] == 1 and com_agents[observation['id']-1][3] == 1: # we're 'head-camper' (and on spot?)
                        sd = 0 #TODO: check 'camping'
                    elif (int(math.ceil(d / MAX_SHOT_DISTANCE)) - 1) <= 0:
                        sd = 0 # we just need to turn
                    else:
                        if dirSame: #TODO: check if fast forward is good
                            sd = d
                        else:
                            sd = d - (MAX_SHOT_DISTANCE - 5)
                    sID = agent['id']
                    sa = a
    if sh == 2:
        #print '%i 2-step targetting! %i camping %i x/y: %s ' % (observation['id'], sd, com_agents[observation['id']-1][2], str(observation['location']))
        return (sh, sa, sd, sID) # targetting a (2 step?) target
        #TODO: return value (how important this target is to shoot)
    return (0, 0, 0, -1) #aw, nothing to shoot :/

def ammo(loc,observation):
    global com_ammo_nav, com_ammo_av
    bestAmmoScore = -999
    bestMovesReq = 99
    bestAmmo = ''
    obsAm = observation['ammo']
    lowestID = -1
    totalAm = 0
    for ag in com_agents:
        totalAm += ag[0]
    if obsAm * 8 < totalAm + 7: # we got less ammo than average in team
        obsAm -= max(3, int(totalAm / 8) - 2) # so we should get more!
    weight = WEIGHT_AM_AMMO * (obsAm - 2) * (abs(obsAm) +1)

    for ammo_com in com_ammo_av:
        amLoc = eval(ammo_com)
        movesReq = 0
        if line_of_sight_old(loc, amLoc):
            d = dist(loc,amLoc)
            a = angle(loc,amLoc)

            a -= observation['direction']
            if abs(a) > math.pi:
                if a >= 0:
                    a -= 2*math.pi
                else:
                    a += 2*math.pi

            movesReq = int(math.ceil(abs(a) / (math.pi / 3))) - 1# turns required
            movesReq += int(math.ceil(d / MAX_SPEED)) # dist steps req
        else:
            #base = - round + com_ammo_av[ammo_com] # decrease with time diff and dist
            movesReq = getHeuristicDist(loc, eval(ammo_com)) #TODO: add move if it's behind us?

        ammoScore = WEIGHT_AM_BASE - weight - WEIGHT_AM_MOVES * movesReq
        if bestAmmoScore < ammoScore and \
                (ammo_com not in com_ammo_nav or movesReq + 2 < com_ammo_nav[ammo_com]):
            bestAmmoScore = ammoScore # we score 50 points higher then prev one going there -> we'll go instead
            bestAmmo = amLoc
            bestMovesReq = movesReq

    # GOTO unvisited areas (LIKELY TO HAVE AMMO with some score)
    i = 0
    if bestAmmoScore < 100:
        for locations in com_locationsVisited:
            [lx, ly, lastR] = locations
            movesReq =  getHeuristicDist(loc, (lx, ly))
            score = 75 + min(100,(round - lastR)*0.3) - (movesReq -3) * movesReq
            amLoc = (lx, ly)
            if bestAmmoScore < score and \
                (str(amLoc) not in com_ammo_nav \
                 or movesReq + 3 < com_ammo_nav[str(amLoc)]):
                lowestID = i
                bestAmmoScore = score
                bestAmmo = (lx, ly)
                bestMovesReq = movesReq
            i += 1
    #print '--going to %s for ammo (location score %i)' % (bestAmmo, com_locationsVisited[lowestID][2])
    #print '\tnew score: %i' %  com_locationsVisited[lowestID][2]

    return (bestAmmoScore, bestAmmo, bestMovesReq, lowestID)

def bestCPGoal(loc,prevGoalID, observation):
    global level, com_controlPScore, com_controlPoints
    bestGoal = -1
    bestScore = -999
    bestHeurDist = 99
    for i in range(3): # for all controlpoints
        conP = level.controls[i]
        heurD = getHeuristicDist(loc, conP)
        scoreP = com_controlPScore[i] - heurD * 2# global - heuristic number of turns to get at point
        if prevGoalID == i: # we had this as goal, let's try to stick to it
            scoreP += WEIGHT_BESTCP_PREV
        if observation['ammo'] < 1:
            if com_controlPoints[i] < -1:
                scoreP -= 400
            if com_controlPoints[i] > 2 and com_agents[observation['id'] -1][3] > 1: # someone else is 'headcamper'
                scoreP -= 200 # camping with no ammo is useless
        if scoreP > bestScore:
            bestScore = scoreP
            bestGoal = i
            bestHeurDist = heurD
    #TODO: TURN THIS IN NICE WEIGHT FUNC GIVEN OBS AND PREV GOAL
    #while dist(loc,level.controls[self.goalID]) < CONTROLPOINT_SIZE:
    #    self.goalID = random.randint(0,2)
    return (bestGoal, bestScore, bestHeurDist)

def dist(a,b):
    x = b[0]-a[0]
    y = b[1]-a[1]
    d = math.sqrt(x**2+y**2)
    return d

def angle(a,b):
    x = b[0]-a[0]
    y = b[1]-a[1]
    a = -math.atan2(-y,x)
    return a


def getHeuristicDist(a, b):
    #Compute the distance between the points + a 'bonus' for different area's
    d = dist(a, b)
    aR = getRegion(a)
    bR = getRegion(b)
    if (aR == 0 and bR == 1): #TODO: add/modify bonus weights?
        d += 500
    elif aR != bR:
        d += 150
    return int(d / MAX_SPEED) # roughly numbers of moves before getting there

#Get the region, pretty hard-coded
def getRegion(p):
    (x,y) = p
    if x > 798 or x < 2 or y > 798 or y < 2:
        return -1 # to prevent going to ammo spawnpoints on walls
    if x >= 600:
        if y < 400:
            return 0 # region A
        return 1 # region B
    if x >= 300:
        return 5 # region F
    if y >= 500:
        return 2 # region C
    if y >= 300:
        return 3 # region D
    return 4 # region E

# Prefered direction when camping
def getPrefDirection(xy):
    (x,y) = xy
    if x > 120 and x < 320:
        if y > 480 and y < 680:
            return 0.75*math.pi
        #elif x == 220 and y == 220:
        return -0.75*math.pi
    if team == 'Red':
        return -0.5*math.pi
    else:
        return 0.5*math.pi

def getFreeCampSpot(goalID, agID, curGoal):
    global com_agents
    curGoal = level.controls[goalID]
    com_agents[agID][1] = goalID

    camperC = 0
    doubleCS = -1
    if dist(com_agents[agID][4], curGoal) <= CONTROLPOINT_SIZE: # we're standing on it.. let's not go to other defend place around it
        com_agents[agID][3] = 1 #TODO: check if other is here too??
        return curGoal

    for i in com_agents:
        if i[2] == 1 and i[1] == goalID: # if controlpoint is the same and it's camping too
            if com_agents[agID][3] == i[3]:
                doubleCS += 1
            if i[3] & camperC == 0:
                camperC += i[3]

    if com_agents[agID][3] == 0 or doubleCS > 0 or 1 & camperC == 0: # we don't have a campspot side yet (or double somehow.. or take 1st spot

        spot = 0
        sI = 1
        if camperC > 0:
            while 1 << spot & camperC != 0: # find the first free 'spot'/position around CP to defend
                spot += 1
            sI = 1 << spot
            #print '%i %i %i C: %i spot is: %i %i ' % (round, self.goalID, observation['id'] - 1, camperC, spot, sI)
        com_agents[agID][3] = sI
    if com_agents[agID][3] > 1:
        curGoal = getCampLoc(goalID, com_agents[agID][3])
        #print '%i %i %i more camping.. take %i def spot.. %s' % (round, self.goalID, observation['id'] - 1, com_agents[observation['id'] - 1][3] >> 1, str(curGoal))
    return curGoal

def getCampLoc(goalID, nr2):
    nr  = nr2 >> 1
    #print 'getCampLoc: %i %i nr2: %i nr: %i' % (x, y, nr2, nr)
    if 1 == goalID: # left-upper point
            if 1 == nr:
                return (280, 160)
            elif 2 == nr:
                return (160, 280)
            elif 3 == nr:
                return (100, 100)
            return (100, 350)
        #580 == y
    elif 2 == goalID:
        if 1 == nr: # left-down
            return (280, 640)
        elif 2 == nr:
            return (160, 520)
        elif 3 == nr:
            return (100, 700)
        return (100, 450)
    if team == 'Red': # 'middle' CP
        if 1 == nr:
            return (440, 340)
        elif 2 == nr:
            return (560, 340)
        elif 3 == nr:
            return (340, 340)
        elif 4 == nr:
            return (580, 200)
        return (500, 50)
    else:
        if 1 == nr:
            return (440, 460)
        elif 2 == nr:
            return (560, 460)
        elif 3 == nr:
            return (340, 460)
        elif 4 == nr:
            return (580, 600)
        return (500, 750)

def processCPObservation(obscp):
    global com_controlPoints, com_controlAgentNearP,com_controlPScore
    for i in range(3): # process control point observations
        if com_controlPoints[i] <= 0 and team == obscp[i]['team']:
            com_controlPoints[i] = 1 # setting it to 2 or 3 happens when that agent checks it
            #print '%s we took control %i' % (team, i)
            if com_controlPScore[i] > 200:
                com_controlPScore[i] = 200
        elif com_controlPoints[i] >= 0 and team != obscp[i]['team'] \
                and 'Neutral' != obscp[i]['team']:
            com_controlPoints[i] = -1 # opponent took the point last turn
            #print '%s we lost control %i' % (team, i)
            # if we saw an enemy at this point last time, we assume it's still their (why -> so we only
            #  go to this point when we got ammo)
            if com_controlPScore[i] < 300:
                com_controlPScore[i] = 300
        elif com_controlPoints[i] > 1 and com_controlAgentNearP[i] == 0:
            #print 'no agents where near %i this one last round, set it back to 1' % i
            com_controlPoints[i] = 1
            #if com_controlPScore[i] < 20:
            #    com_controlPScore[i] = 20

        if com_controlPScore[i] > 400:
            com_controlPScore[i] = 400
        if com_controlPScore[i] > 250 and com_controlPoints[i] > 0:
            com_controlPScore[i] = 250

    com_controlAgentNearP = [0, 0, 0]
    #print com_controlPScore

def showlevel(start, goal):
    #global level
    #agent = AgentBrain()
    #path = agent.theta_star(start, goal, False)


    pygame.init()
    window = pygame.display.set_mode((800, 800))
    displayUpdate(level.tree, window)
    pygame.draw.circle(window, (0, 255, 255), start, 5)
    pygame.draw.circle(window, (255, 255, 0), goal, 5)
    print "start:",start
    print "goal:",goal
#    if not path is None:
#        nextpath = path.parent
#
#        while(path != nextpath):
#            pygame.draw.line(window, (102, 194, 33), path.loc(), nextpath.loc())
#            path = nextpath
#            nextpath = path.parent
#        pygame.display.update()

    for w in level.walls:
        pygame.draw.line(window, (255, 0, 0), w[0], w[1])
    for wx in tempWalls:
        for w in wx:
            pygame.draw.line(window, (0, 0, 255), w[0], w[1])
        for w in tempWalls[wx]:
            pygame.draw.line(window, (0, 255, 0), w[0], w[1])
    pygame.display.update()

    while True:
       for event in pygame.event.get():
            if event.type == pygame.QUIT:
                return

def displayUpdate(tree,window):
    leafs = tree.getLeafs()
    for leaf in leafs:
        if (leaf.points['nw'].neigh['e']):
            pygame.draw.line(window, (255, 255, 255), leaf.points['nw'].loc(), leaf.points['nw'].neigh['e'].loc())
        if (leaf.points['ne'].neigh['s']):
            pygame.draw.line(window, (255, 255, 255), leaf.points['ne'].loc(), leaf.points['ne'].neigh['s'].loc())
        if (leaf.points['se'].neigh['w']):
            pygame.draw.line(window, (255, 255, 255), leaf.points['se'].loc(), leaf.points['se'].neigh['w'].loc())
        if leaf.points['sw'].neigh['n']:
            pygame.draw.line(window, (255, 255, 255), leaf.points['sw'].loc(), leaf.points['sw'].neigh['n'].loc())

    pygame.display.update()

class ASTree:
    def __init__(self, width, height):
        nw = Point(-5,-5)
        ne = Point(width+5, -5)
        se = Point(width+5, height+5)
        sw = Point(-5, width+5)
        nw.neigh['e'] = ne
        nw.neigh['s'] = sw
        ne.neigh['w'] = nw
        ne.neigh['s'] = se
        se.neigh['n'] = ne
        se.neigh['w'] = sw
        sw.neigh['e'] = se
        sw.neigh['n'] = nw
        self.root = Area(nw, ne, se, sw)

    def addSplit(self, x, y, t=False):
        return self.root.addSplit(x, y, t)

    def getLeafs(self):
        return self.root.getSubAreas()

    def addLineSplit(self,s,o,t=False):
        self.root.addLineSplit(s,o,t)

    def cleanMap(self):
        self.root.cleanTemps()

class Area:
    def __init__(self,nw,ne,se,sw,parent=None):
        self.points = {'nw':nw, 'ne':ne, 'se':se, 'sw':sw}
        self.parent = parent
        self.split = None
        self.subArea = {'nw':None, 'ne':None, 'se':None, 'sw':None}
        self.check = [False,False]

    def getArea(self,point):
        if self.split is None:
            return [self]
        else:
            if point.x == self.split.x:
                if point.y == self.split.y:
                    t = []
                    if self.subArea['nw'] != None:
                        t += self.subArea['nw'].getArea(point)
                    if self.subArea['ne'] != None:
                        t += self.subArea['ne'].getArea(point)
                    if self.subArea['se'] != None:
                        t += self.subArea['se'].getArea(point)
                    if self.subArea['sw'] != None:
                        t += self.subArea['sw'].getArea(point)
                    return t
                elif point.y > self.split.y:
                    t = []
                    if self.subArea['se'] != None:
                        t += self.subArea['se'].getArea(point)
                    if self.subArea['sw'] != None:
                        t += self.subArea['sw'].getArea(point)
                    return t
                else:
                    t = []
                    if self.subArea['ne'] != None:
                        t += self.subArea['ne'].getArea(point)
                    if self.subArea['nw'] != None:
                        t += self.subArea['nw'].getArea(point)
                    return t
            elif point.x > self.split.x:
                if point.y == self.split.y:
                    t = []
                    if self.subArea['se'] != None:
                        t += self.subArea['se'].getArea(point)
                    if self.subArea['ne'] != None:
                        t += self.subArea['ne'].getArea(point)
                    return t
                elif point.y > self.split.y:
                    t = []
                    if self.subArea['se'] != None:
                        t += self.subArea['se'].getArea(point)
                    return t
                else:
                    t = []
                    if self.subArea['ne'] != None:
                        t += self.subArea['ne'].getArea(point)
                    return t
            elif point.x < self.split.x:
                if point.y == self.split.y:
                    t = []
                    if self.subArea['sw'] != None:
                        t += self.subArea['sw'].getArea(point)
                    if self.subArea['nw'] != None:
                        t += self.subArea['nw'].getArea(point)
                    return t
                elif point.y > self.split.y:
                    t = []
                    if self.subArea['sw'] != None:
                        t += self.subArea['sw'].getArea(point)
                    return t
                else:
                    t = []
                    if self.subArea['nw'] != None:
                        t += self.subArea['nw'].getArea(point)
                    return t

    def addSplit(self,x,y,temp):
        point = Point(x,y)
        areas = self.getArea(point)

        if len(areas) == 0:
            pass
        elif len(areas) == 1:
            area = areas[0]
            area.split = point
            #Create points
            n = Point(point.x, area.points['nw'].y)
            w = Point(area.points['nw'].x, point.y)
            s = Point(point.x, area.points['se'].y)
            e = Point(area.points['se'].x, point.y)
            if temp:
                area.check[1] = True
                a1 = area.parent
                while a1 != None:
                    a1.check[0] = True
                    a1 = a1.parent
                point.temp = True
                n.temp = True
                n.recon = ['w','e']
                w.temp = True
                w.recon = ['n','s']
                s.temp = True
                s.recon = ['w','e']
                e.temp = True
                e.recon = ['n','s']
            #Create Areas (nw,ne,se,sw,parent)
            area.subArea['nw'] = Area(area.points['nw'],n,point,w,area)
            area.subArea['ne'] = Area(n,area.points['ne'],e,point,area)
            area.subArea['se'] = Area(point,e,area.points['se'],s,area)
            area.subArea['sw'] = Area(w,point,s,area.points['sw'],area)
            #Fix connections
            point.neigh['n'] = n
            point.neigh['w'] = w
            point.neigh['s'] = s
            point.neigh['e'] = e
            n.neigh['s'] = point
            w.neigh['e'] = point
            s.neigh['n'] = point
            e.neigh['w'] = point
            (p1,p2) = area.points['nw'].getInterval('e',point)
            p1.neigh['e'] = n
            p2.neigh['w'] = n
            n.neigh['w'] = p1
            n.neigh['e'] = p2
            (p1,p2) = area.points['ne'].getInterval('s',point)
            p1.neigh['s'] = e
            p2.neigh['n'] = e
            e.neigh['n'] = p1
            e.neigh['s'] = p2
            (p1,p2) = area.points['se'].getInterval('w',point)
            p1.neigh['w'] = s
            p2.neigh['e'] = s
            s.neigh['e'] = p1
            s.neigh['w'] = p2
            (p1,p2) = area.points['sw'].getInterval('n',point)
            p1.neigh['n'] = w
            p2.neigh['s'] = w
            w.neigh['s'] = p1
            w.neigh['n'] = p2
            return point
        elif len(areas) == 2:
            areas[0].split = point
            areas[1].split = point
            if areas[0].points['nw'].x == point.x:
                n = areas[0].points['nw']
                w = Point(areas[1].points['nw'].x, point.y)
                s = areas[1].points['se']
                e = Point(areas[0].points['se'].x, point.y)
                if temp:
                    for i in [0,1]:
                        areas[i].check[1] = True
                        a1 = areas[i].parent
                        while a1 != None:
                            a1.check[0] = True
                            a1 = a1.parent
                    point.temp = True
                    point.recon = ['n','s']
                    w.temp = True
                    w.recon = ['n','s']
                    e.temp = True
                    e.recon = ['n','s']
                #Create Areas (nw,ne,se,sw,parent)
                areas[0].subArea['ne'] = Area(n,areas[0].points['ne'],e,point,areas[0])
                areas[0].subArea['se'] = Area(point,e,areas[0].points['se'],areas[0].points['sw'],areas[0])
                areas[1].subArea['nw'] = Area(areas[1].points['nw'],areas[1].points['ne'],point,w,areas[1])
                areas[1].subArea['sw'] = Area(w,point,s,areas[1].points['sw'],areas[1])
                #Fix connections
                point.neigh['w'] = w
                point.neigh['e'] = e
                w.neigh['e'] = point
                e.neigh['w'] = point
                (p1,p2) = areas[0].points['nw'].getInterval('s',point)
                p1.neigh['s'] = point
                p2.neigh['n'] = point
                point.neigh['n'] = p1
                point.neigh['s'] = p2
                (p1,p2) = areas[0].points['se'].getInterval('n',point)
                p1.neigh['n'] = e
                p2.neigh['s'] = e
                e.neigh['s'] = p1
                e.neigh['n'] = p2
                (p1,p2) = areas[1].points['nw'].getInterval('s',point)
                p1.neigh['s'] = w
                p2.neigh['n'] = w
                w.neigh['n'] = p1
                w.neigh['s'] = p2
                return point
            elif areas[0].points['nw'].y == point.y:
                n = Point(point.x, areas[1].points['nw'].y)
                w = areas[0].points['nw']
                s = Point(point.x, areas[0].points['se'].y)
                e = areas[1].points['se']
                if temp:
                    for i in [0,1]:
                        areas[i].check[1] = True
                        a1 = areas[i].parent
                        while a1 != None:
                            a1.check[0] = True
                            a1 = a1.parent
                    point.temp = True
                    point.recon = ['w','e']
                    n.temp = True
                    n.recon = ['w','e']
                    s.temp = True
                    s.recon = ['w','e']
                #Create Areas (nw,ne,se,sw,parent)
                areas[0].subArea['sw'] = Area(w,point,s,areas[0].points['sw'],areas[0])
                areas[0].subArea['se'] = Area(point,areas[0].points['ne'],areas[0].points['se'],s,areas[0])
                areas[1].subArea['nw'] = Area(areas[1].points['nw'],n,point,areas[1].points['sw'],areas[1])
                areas[1].subArea['ne'] = Area(n,areas[1].points['ne'],e,point,areas[1])
                #Fix connections
                point.neigh['n'] = n
                point.neigh['s'] = s
                n.neigh['s'] = point
                s.neigh['n'] = point
                (p1,p2) = areas[0].points['nw'].getInterval('e',point)
                p1.neigh['e'] = point
                p2.neigh['w'] = point
                point.neigh['w'] = p1
                point.neigh['e'] = p2
                (p1,p2) = areas[0].points['se'].getInterval('w',point)
                p1.neigh['w'] = s
                p2.neigh['e'] = s
                s.neigh['e'] = p1
                s.neigh['w'] = p2
                (p1,p2) = areas[1].points['nw'].getInterval('e',point)
                p1.neigh['e'] = n
                p2.neigh['w'] = n
                n.neigh['w'] = p1
                n.neigh['e'] = p2
                return point
            elif areas[0].points['se'].x == point.x:
                n = areas[1].points['nw']
                w = Point(areas[0].points['nw'].x, point.y)
                s = areas[0].points['se']
                e = Point(areas[1].points['se'].x, point.y)
                if temp:
                    for i in [0,1]:
                        areas[i].check[1] = True
                        a1 = areas[i].parent
                        while a1 != None:
                            a1.check[0] = True
                            a1 = a1.parent
                    point.temp = True
                    point.recon = ['n','s']
                    w.temp = True
                    w.recon = ['n','s']
                    e.temp = True
                    e.recon = ['n','s']
                #Create Areas (nw,ne,se,sw,parent)
                areas[1].subArea['ne'] = Area(n,areas[1].points['ne'],e,point,areas[1])
                areas[1].subArea['se'] = Area(point,e,areas[1].points['se'],areas[1].points['sw'],areas[1])
                areas[0].subArea['nw'] = Area(areas[0].points['nw'],areas[0].points['ne'],point,w,areas[0])
                areas[0].subArea['sw'] = Area(w,point,s,areas[0].points['sw'],areas[0])
                #Fix connections
                point.neigh['w'] = w
                point.neigh['e'] = e
                w.neigh['e'] = point
                e.neigh['w'] = point
                (p1,p2) = areas[1].points['nw'].getInterval('s',point)
                p1.neigh['s'] = point
                p2.neigh['n'] = point
                point.neigh['n'] = p1
                point.neigh['s'] = p2
                (p1,p2) = areas[1].points['se'].getInterval('n',point)
                p1.neigh['n'] = e
                p2.neigh['s'] = e
                e.neigh['s'] = p1
                e.neigh['n'] = p2
                (p1,p2) = areas[0].points['nw'].getInterval('s',point)
                p1.neigh['s'] = w
                p2.neigh['n'] = w
                w.neigh['n'] = p1
                w.neigh['s'] = p2
                return point
            elif areas[0].points['se'].y == point.y:
                n = Point(point.x, areas[0].points['nw'].y)
                w = areas[1].points['nw']
                s = Point(point.x, areas[1].points['se'].y)
                e = areas[0].points['se']
                if temp:
                    for i in [0,1]:
                        areas[i].check[1] = True
                        a1 = areas[i].parent
                        while a1 != None:
                            a1.check[0] = True
                            a1 = a1.parent
                    point.temp = True
                    point.recon = ['w','e']
                    n.temp = True
                    n.recon = ['w','e']
                    s.temp = True
                    s.recon = ['w','e']
                #Create Areas (nw,ne,se,sw,parent)
                areas[1].subArea['sw'] = Area(w,point,s,areas[1].points['sw'],areas[1])
                areas[1].subArea['se'] = Area(point,areas[1].points['ne'],areas[1].points['se'],s,areas[1])
                areas[0].subArea['nw'] = Area(areas[0].points['nw'],n,point,areas[0].points['sw'],areas[0])
                areas[0].subArea['ne'] = Area(n,areas[0].points['ne'],e,point,areas[0])
                #Fix connections
                point.neigh['n'] = n
                point.neigh['s'] = s
                n.neigh['s'] = point
                s.neigh['n'] = point
                (p1,p2) = areas[1].points['nw'].getInterval('e',point)
                p1.neigh['e'] = point
                p2.neigh['w'] = point
                point.neigh['w'] = p1
                point.neigh['e'] = p2
                (p1,p2) = areas[1].points['se'].getInterval('w',point)
                p1.neigh['w'] = s
                p2.neigh['e'] = s
                s.neigh['e'] = p1
                s.neigh['w'] = p2
                (p1,p2) = areas[0].points['nw'].getInterval('e',point)
                p1.neigh['e'] = n
                p2.neigh['w'] = n
                n.neigh['w'] = p1
                n.neigh['e'] = p2
                return point
        elif len(areas) == 3:
            corners = ['nw', 'ne', 'sw', 'se']
            t = []
            for a in areas:
                for c in corners:
                    if a.points[c].x == point.x and a.points[c].y == point.y:
                        t.append((a,c))
            if len(t) != 2:
                print "Error"
            else:
                areas.remove(t[0][0])
                areas.remove(t[1][0])
                areas[0].split = t[0][0].points[t[0][1]]
                if t[0][1][0] == t[1][1][0]:
                    if t[0][1][0] == 'n':
                        n = Point(point.x,areas[0].points['nw'].y)
                        if temp:
                            areas[0].check[1] = True
                            a1 = areas[0].parent
                            while a1 != None:
                                a1.check[0] = True
                                a1 = a1.parent
                            n.temp = True
                            n.recon = ['w','e']
                        #Create Areas (nw,ne,se,sw,parent)
                        areas[0].subArea['nw'] = Area(areas[0].points['nw'], n, t[0][0].points[t[0][1]], areas[0].points['sw'], areas[0])
                        areas[0].subArea['ne'] = Area(n, areas[0].points['ne'], areas[0].points['se'], t[0][0].points[t[0][1]], areas[0])
                        #Fix connections
                        t[0][0].points[t[0][1]].neigh['n'] = n
                        n.neigh['s'] = t[0][0].points[t[0][1]]

                        (p1,p2) = areas[0].points['nw'].getInterval('e', n)
                        p1.neigh['e'] = n
                        p2.neigh['w'] = n
                        n.neigh['w'] = p1
                        n.neigh['e'] = p2
                        return t[0][0].points[t[0][1]]
                    elif t[0][1][0] == 's':
                        s = Point(point.x,areas[0].points['sw'].y)
                        if temp:
                            areas[0].check[1] = True
                            a1 = areas[0].parent
                            while a1 != None:
                                a1.check[0] = True
                                a1 = a1.parent
                            s.temp = True
                            s.recon = ['w','e']
                        #Create Areas (nw,ne,se,sw,parent)
                        areas[0].subArea['sw'] = Area(areas[0].points['nw'], t[0][0].points[t[0][1]], s, areas[0].points['sw'], areas[0])
                        areas[0].subArea['se'] = Area(t[0][0].points[t[0][1]], areas[0].points['ne'], areas[0].points['se'], s, areas[0])

                        #Fix connections
                        t[0][0].points[t[0][1]].neigh['s'] = s
                        s.neigh['n'] = t[0][0].points[t[0][1]]

                        (p1,p2) = areas[0].points['sw'].getInterval('e', s)
                        p1.neigh['e'] = s
                        p2.neigh['w'] = s
                        s.neigh['w'] = p1
                        s.neigh['e'] = p2
                        return t[0][0].points[t[0][1]]
                    else:
                        print "Error"
                elif t[0][1][1] == t[1][1][1]:
                    if t[0][1][1] == 'w':
                        w = Point(areas[0].points['nw'].x, point.y)
                        if temp:
                            areas[0].check[1] = True
                            a1 = areas[0].parent
                            while a1 != None:
                                a1.check[0] = True
                                a1 = a1.parent
                            w.temp = True
                            w.recon = ['n','s']
                        #Create Areas (nw,ne,se,sw,parent)
                        areas[0].subArea['nw'] = Area(areas[0].points['nw'], areas[0].points['ne'], t[0][0].points[t[0][1]], w, areas[0])
                        areas[0].subArea['sw'] = Area(w, t[0][0].points[t[0][1]], areas[0].points['se'], areas[0].points['sw'], areas[0])
                        #Fix connections
                        t[0][0].points[t[0][1]].neigh['w'] = w
                        w.neigh['e'] = t[0][0].points[t[0][1]]

                        (p1,p2) = areas[0].points['nw'].getInterval('s', w)
                        p1.neigh['s'] = w
                        p2.neigh['n'] = w
                        w.neigh['n'] = p1
                        w.neigh['s'] = p2
                        return t[0][0].points[t[0][1]]
                    elif t[0][1][1] == 'e':
                        e = Point(areas[0].points['ne'].x, point.y)
                        if temp:
                            areas[0].check[1] = True
                            a1 = areas[0].parent
                            while a1 != None:
                                a1.check[0] = True
                                a1 = a1.parent
                            e.temp = True
                            e.recon = ['n','s']
                        #Create Areas (nw,ne,se,sw,parent)
                        areas[0].subArea['ne'] = Area(areas[0].points['nw'], areas[0].points['ne'], e, t[0][0].points[t[0][1]], areas[0])
                        areas[0].subArea['se'] = Area(t[0][0].points[t[0][1]], e, areas[0].points['se'], areas[0].points['sw'], areas[0])

                        #Fix connections
                        t[0][0].points[t[0][1]].neigh['e'] = e
                        e.neigh['w'] = t[0][0].points[t[0][1]]

                        (p1,p2) = areas[0].points['se'].getInterval('n', e)
                        p1.neigh['n'] = e
                        p2.neigh['s'] = e
                        e.neigh['s'] = p1
                        e.neigh['n'] = p2
                        return t[0][0].points[t[0][1]]
                    else:
                        print "Error"
                else:
                    print "Error"
        elif len(areas) == 4:
            for k,v in areas[0].points.iteritems():
                c = 0
                for a in areas[1:]:
                    for k2,v2 in a.points.iteritems():
                        if v2.x == v.x and v2.y == v.y:
                            c += 1
                            break
                if c == 3:
                    return v
            print '\n\n\n\nFAIL\n\n\n\n'
            if temp:
                point.temp = True
            return point

    def getSubAreas(self):
        if self.split is None:
            return [self]
        t = []
        for key in self.subArea:
            if  not (self.subArea[key] is None):
                t = t + self.subArea[key].getSubAreas()
        return t

    def addLineSplit(self, start, offset,temp):
        if  offset[0] > 0 and offset[1] == 0:
            p = self.addSplit(start[0], start[1],temp)
            while (p.neigh['e'].x < start[0]+offset[0]):
                p = self.addSplit(p.neigh['e'].x, p.neigh['e'].y,temp)
            self.addSplit(start[0]+offset[0], start[1],temp)
        elif  offset[0] == 0 and offset[1] > 0:
            p = self.addSplit(start[0], start[1],temp)
            while (p.neigh['s'].y < start[1]+offset[1]):
                p = self.addSplit(p.neigh['s'].x, p.neigh['s'].y,temp)
            self.addSplit(start[0], start[1]+offset[1],temp)
        else:
            print 'Invalid line definition'

    def cleanTemps(self):
        if self.check[0]:
            for k in self.subArea:
                if self.subArea[k] != None:
                    self.subArea[k].cleanTemps()
            self.check[0] = False
        if self.check[1]:
            for k in self.split.neigh:
                if self.split.neigh[k] != None:
                    self.split.neigh[k].reconnect()
            self.split.reconnect()
            self.split = None
            for k in self.subArea:
                if self.subArea[k] != None:
                    for k2 in self.subArea[k].points:
                        self.subArea[k].points[k2] = None
                    self.subArea[k] = None
            self.check[1] = False

class Point:
    def __init__(self, x=None, y=None):
        """AStree data"""
        self.x = x
        self.y = y
        self.neigh = {}
        self.temp = False
        self.recon = []

        """Theta * data"""
        self.x = x
        self.y = y
        self.val = 0
        self.g = 0
        self.parent = None

    def __eq__(self, other):
        return (self.x == other.x and self.y == other.y)

    def __lt__(self, other):
        return self.val < other.val

    def getInterval(self,dir,point):
        if dir == 'n':
            if (point.y >= self.neigh['n'].y):
                return (self,self.neigh['n'])
            else:
                return self.neigh['n'].getInterval(dir, point)
        elif dir == 'w':
            if (point.x >= self.neigh['w'].x):
                return (self,self.neigh['w'])
            else:
                return self.neigh['w'].getInterval(dir, point)
        elif dir == 's':
            if (point.y <= self.neigh['s'].y):
                return (self,self.neigh['s'])
            else:
                return self.neigh['s'].getInterval(dir, point)
        elif dir == 'e':
            if (point.x <= self.neigh['e'].x):
                return (self,self.neigh['e'])
            else:
                return self.neigh['e'].getInterval(dir, point)

    def loc(self):
        return (self.x, self.y)

    def reconnect(self):
        if self.temp:
            op = {'n':'s','w':'e','s':'n','e':'w'}
            for k in op:
                if k in self.neigh and not (self.neigh[k] is None):
                    if k in self.recon:
                        self.neigh[k].neigh[op[k]] = self.neigh[op[k]]
                    else:
                        self.neigh[k].neigh[op[k]] = None
            for k in op:
                if k in self.neigh:
                    self.neigh[k] = None

class ReadLevel:

    def __init__(self,levelLoc):
        f = open(levelLoc, 'r')
        ls = f.readlines()
        f.close()
        t = []
        f = False
        self.controls = []
        for l in ls:
            s = l.split()

            if len(s) != 0 and s[0] == 'WALLS':
                t.extend(s[2:])
                f = True
            elif f and s[0][0] == '#':
                pass
            elif f and "]" in s:
                t.extend(s)
                f = False
            elif f:
                t.extend(s)
            elif len(s) != 0 and s[0] == 'CONTROL_POINTS':
                self.controls = eval("".join(s[2:]))
            else:
                pass

        self.wallList = eval("".join(t))

    def createTree(self):
        controlsCopy = self.controls[:]
        self.walls = []
        self.tree = ASTree(800, 800)
        for w in self.wallList:
           # w = self.wallList.pop()
            #Fix offsets
            a = (w[0][0]-6, w[0][1]-6)
            a1 = (w[0][0]-5, w[0][1]-5)
            o = (w[1][0]+12, w[1][1]+12)
            o1 = (w[1][0]+10, w[1][1]+10)

            b = (a[0]+o[0], a[1]+o[1])
            b1 = (a1[0]+o1[0], a1[1]+o1[1])
            c = (a[0]+o[0], a[1])
            c1 = (a1[0]+o1[0], a1[1])
            d = (a[0], a[1]+o[1])
            d1 = (a1[0], a1[1]+o1[1])

            self.walls.append((a1,b1))
            self.walls.append((c1,d1))
            #corners.extend([a,b,c,d])

            self.tree.addLineSplit(a,(o[0],0))
            self.tree.addLineSplit(a,(0,o[1]))
            self.tree.addLineSplit(d,(o[0],0))
            self.tree.addLineSplit(c,(0,o[1]))
        for c in controlsCopy:
            #c = controlsCopy.pop()
            self.tree.addSplit(c[0],c[1])
